#include "MarchingTetrahedrons.h"

int MarchingTetrahedrons::PolygoniseTri(GRIDCELL g,double iso, TRIANGLE *tri,int v0,int v1,int v2,int v3){
   int ntri = 0 , triindex = 0;

   /*
      Determine which of the 16 cases we have given which vertices
      are above or below the isosurface
   */
   if (g.val[v0] < iso) triindex |= 1;
   if (g.val[v1] < iso) triindex |= 2;
   if (g.val[v2] < iso) triindex |= 4;
   if (g.val[v3] < iso) triindex |= 8;

   /* Form the vertices of the triangles for each case. Tests the orientation of the triangles. The orientation of a
    triangle is set to be the oposite of its "dual" case.
    */
   switch (triindex) {
   case 0x00:
   case 0x0F:
      break;
   case 0x0E:
      tri[0].p[0] = VertexInterp(iso,g.p[v0],g.p[v1],g.val[v0],g.val[v1]);
      tri[0].p[1] = VertexInterp(iso,g.p[v0],g.p[v2],g.val[v0],g.val[v2]);
      tri[0].p[2] = VertexInterp(iso,g.p[v0],g.p[v3],g.val[v0],g.val[v3]);
      ntri++;
      break;
   case 0x01:
      tri[0].p[0] = VertexInterp(iso,g.p[v0],g.p[v3],g.val[v0],g.val[v3]);
      tri[0].p[1] = VertexInterp(iso,g.p[v0],g.p[v2],g.val[v0],g.val[v2]);
      tri[0].p[2] = VertexInterp(iso,g.p[v0],g.p[v1],g.val[v0],g.val[v1]);
      ntri++;
      break;
   case 0x0D:
      tri[0].p[0] = VertexInterp(iso,g.p[v1],g.p[v0],g.val[v1],g.val[v0]);
      tri[0].p[1] = VertexInterp(iso,g.p[v1],g.p[v3],g.val[v1],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v1],g.p[v2],g.val[v1],g.val[v2]);
      ntri++;
      break;
   case 0x02:
      tri[0].p[0] = VertexInterp(iso,g.p[v1],g.p[v2],g.val[v1],g.val[v2]);
      tri[0].p[1] = VertexInterp(iso,g.p[v1],g.p[v3],g.val[v1],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v1],g.p[v0],g.val[v1],g.val[v0]);
      ntri++;
      break;
   case 0x0C:
      tri[0].p[0] = VertexInterp(iso,g.p[v0],g.p[v3],g.val[v0],g.val[v3]);
      tri[0].p[1] = VertexInterp(iso,g.p[v1],g.p[v3],g.val[v1],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v0],g.p[v2],g.val[v0],g.val[v2]);
      ntri++;
      tri[1].p[0] = VertexInterp(iso,g.p[v1],g.p[v2],g.val[v1],g.val[v2]);
      tri[1].p[1] = tri[0].p[2];
      tri[1].p[2] = tri[0].p[1];
      ntri++;
      break;
   case 0x03:
      tri[0].p[0] = VertexInterp(iso,g.p[v0],g.p[v2],g.val[v0],g.val[v2]);
      tri[0].p[1] = VertexInterp(iso,g.p[v1],g.p[v3],g.val[v1],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v0],g.p[v3],g.val[v0],g.val[v3]);
      ntri++;
      tri[1].p[0] = VertexInterp(iso,g.p[v1],g.p[v2],g.val[v1],g.val[v2]);
      tri[1].p[1] = tri[0].p[1];
      tri[1].p[2] = tri[0].p[0];
      ntri++;
      break;
   case 0x0B:
      tri[0].p[0] = VertexInterp(iso,g.p[v2],g.p[v0],g.val[v2],g.val[v0]);
      tri[0].p[1] = VertexInterp(iso,g.p[v2],g.p[v1],g.val[v2],g.val[v1]);
      tri[0].p[2] = VertexInterp(iso,g.p[v2],g.p[v3],g.val[v2],g.val[v3]);
      ntri++;
      break;
   case 0x04:
      tri[0].p[0] = VertexInterp(iso,g.p[v2],g.p[v3],g.val[v2],g.val[v3]);
      tri[0].p[1] = VertexInterp(iso,g.p[v2],g.p[v1],g.val[v2],g.val[v1]);
      tri[0].p[2] = VertexInterp(iso,g.p[v2],g.p[v0],g.val[v2],g.val[v0]);
      ntri++;
      break;
   case 0x0A:
      tri[0].p[0] = VertexInterp(iso,g.p[v0],g.p[v1],g.val[v0],g.val[v1]);
      tri[0].p[1] = VertexInterp(iso,g.p[v2],g.p[v3],g.val[v2],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v0],g.p[v3],g.val[v0],g.val[v3]);
      ntri++;
      tri[1].p[0] = tri[0].p[1];
      tri[1].p[1] = tri[0].p[0];
      tri[1].p[2] = VertexInterp(iso,g.p[v1],g.p[v2],g.val[v1],g.val[v2]);
      ntri++;
      break;
   case 0x05:
      tri[0].p[0] = VertexInterp(iso,g.p[v0],g.p[v3],g.val[v0],g.val[v3]);
      tri[0].p[1] = VertexInterp(iso,g.p[v2],g.p[v3],g.val[v2],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v0],g.p[v1],g.val[v0],g.val[v1]);
      ntri++;
      tri[1].p[0] = VertexInterp(iso,g.p[v1],g.p[v2],g.val[v1],g.val[v2]);
      tri[1].p[1] = tri[0].p[2];
      tri[1].p[2] = tri[0].p[1];
      ntri++;
      break;
   case 0x09:
      tri[0].p[0] = VertexInterp(iso,g.p[v0],g.p[v1],g.val[v0],g.val[v1]);
      tri[0].p[1] = VertexInterp(iso,g.p[v1],g.p[v3],g.val[v1],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v2],g.p[v3],g.val[v2],g.val[v3]);
      ntri++;
      tri[1].p[0] = tri[0].p[0];
      tri[1].p[1] = tri[0].p[2];
      tri[1].p[2] = VertexInterp(iso,g.p[v0],g.p[v2],g.val[v0],g.val[v2]);
      ntri++;
      break;
   case 0x06:
      tri[0].p[0] = VertexInterp(iso,g.p[v2],g.p[v3],g.val[v2],g.val[v3]);
      tri[0].p[1] = VertexInterp(iso,g.p[v1],g.p[v3],g.val[v1],g.val[v3]);
      tri[0].p[2] = VertexInterp(iso,g.p[v0],g.p[v1],g.val[v0],g.val[v1]);
      ntri++;
      tri[1].p[0] = tri[0].p[0];
      tri[1].p[1] = tri[0].p[2];
      tri[1].p[2] = VertexInterp(iso,g.p[v0],g.p[v2],g.val[v0],g.val[v2]);
      ntri++;
      break;
   case 0x07:
      tri[0].p[0] = VertexInterp(iso,g.p[v3],g.p[v0],g.val[v3],g.val[v0]);
      tri[0].p[1] = VertexInterp(iso,g.p[v3],g.p[v2],g.val[v3],g.val[v2]);
      tri[0].p[2] = VertexInterp(iso,g.p[v3],g.p[v1],g.val[v3],g.val[v1]);
      ntri++;
      break;
   case 0x08:
      tri[0].p[0] = VertexInterp(iso,g.p[v3],g.p[v1],g.val[v3],g.val[v1]);
      tri[0].p[1] = VertexInterp(iso,g.p[v3],g.p[v2],g.val[v3],g.val[v2]);
      tri[0].p[2] = VertexInterp(iso,g.p[v3],g.p[v0],g.val[v3],g.val[v0]);
      ntri++;
      break;
   }

   for(int i = 0 ; i < ntri ; i++){
       calcNormal(tri[i]);
       tri[i].type = triindex; //debugging only...
   }
   return(ntri);
}

XYZ MarchingTetrahedrons::VertexInterp(double isolevel , XYZ p1 , XYZ p2 , double valp1 , double valp2)
{
   /*
   Linearly interpolate the position where an isosurface cuts
   an edge between two vertices, each with their own scalar value
    */

   double mu;
   XYZ p;

   if (ABS(isolevel-valp1) < 0.00001)
      return(p1);
   if (ABS(isolevel-valp2) < 0.00001)
      return(p2);
   if (ABS(valp1-valp2) < 0.00001)
      return(p1);

   mu = (isolevel - valp1) / (valp2 - valp1);
   p.x = p1.x + mu * (p2.x - p1.x);
   p.y = p1.y + mu * (p2.y - p1.y);
   p.z = p1.z + mu * (p2.z - p1.z);

   return(p);
}

void MarchingTetrahedrons::calcNormal(TRIANGLE &tri){
    //Calculate the normal vector of the triangle.
    double norm;

    XYZ p0 = tri.p[0];
    XYZ p1 = tri.p[1];
    XYZ p2 = tri.p[2];
    Plane plane(Point(p0.x , p0.y , p0.z) , Point(p1.x , p1.y , p1.z) , Point(p2.x , p2.y , p2.z));
    Vector vector(plane.orthogonal_vector());

    norm = sqrt(vector.x() * vector.x() + vector.y() * vector.y() + vector.z() * vector.z());
    vector = vector / norm;

    tri.normal.x = vector.x();
    tri.normal.y = vector.y();
    tri.normal.z = vector.z();
}
